#!/usr/bin/env python3
# -*- coding: utf-8 -*-

from interface import *
import time

# 等待 ROS 进程启动完成
time.sleep(10)

# 初始化 tecbot
initialized = False
init(initpose_x=0, initpose_y=0, initpose_yaw=0)

# 更新心跳包数据


def pose_callback(msg):
    assert isinstance(msg, geometry_msgs.msg.PoseWithCovarianceStamped)
    hal.protocol.set_flow_state(x=round(msg.pose.pose.position.x, 3),
                                y=round(msg.pose.pose.position.y, 3),
                                yaw=round(hal.utils.deg_from_rad(hal.utils.yaw_from_quaternion(
                                    msg.pose.pose.orientation)), 3))
    global initialized
    if not initialized:
        hal.protocol.set_flow_state(state=symbols.flow_state_code['reached'])
        initialized = True


def battery_callback(msg):
    assert isinstance(msg, battery_state)
    hal.protocol.set_flow_state(voltage=msg.voltage, percent=msg.soc)


_ = rospy.Subscriber("tecbot/localization/pose",
                     geometry_msgs.msg.PoseWithCovarianceStamped, pose_callback, queue_size=1)
_ = rospy.Subscriber("tecbot/hardware/bms",
                     battery_state, battery_callback, queue_size=1)


# 循环频率
rate = rospy.Rate(10.0)

while not rospy.is_shutdown():
    if not rospy.is_shutdown():
        try:
            try:
                current_state = rospy.wait_for_message(
                    'tecbot/navigation/state', nav_to_state, timeout=0.2)
                assert isinstance(current_state, nav_to_state)
                hal.protocol.set_flow_state(state=current_state.state)
            except rospy.exceptions.ROSException:
                pass

            rate.sleep()
        except rospy.exceptions.ROSException:
            pass
    else:
        break
